import genesis as gs
import numpy as np

gs.init(backend=gs.gpu)

viewer_options = gs.options.ViewerOptions(
    camera_pos=(0, -3.5, 2.5),
    camera_lookat=(0.0, 0.0, 0.5),
    camera_fov=40,
    max_FPS=60,
)

scene = gs.Scene(
    viewer_options=viewer_options,
    sim_options=gs.options.SimOptions(
        dt=0.01,
        substeps=1,
        gravity=(0, 0, -10),
    ),
    show_viewer=True,
    renderer=gs.renderers.Rasterizer(),
)

ground = scene.add_entity(
    gs.morphs.Plane()
)

for i in range(4):
    for j in range(4):
        franka = scene.add_entity(
            gs.morphs.MJCF(
                file="xml/franka_emika_panda/panda.xml",
                pos = (i * 2, j * 2, 0),
                euler = np.random.rand(3) * 2 * np.pi,
                scale = 1.0,          
            ),
        )
        target_sphere = scene.add_entity(
            gs.morphs.Sphere(
                radius=0.05,
                pos=(i * 2 + np.random.rand(), j * 2  + np.random.rand(), 0)
            ),
        )


scene.build()

while True:
    scene.step()